#include "ground.h"
#include <sstream>

Ground::Ground(const Cube& cube,const Point& dir,const ObjectModelPtr& model)
	:	SceneObject(cube,dir,model) , Solid(cube){
}

Ground::~Ground(){
}

void Ground::notifySolidPositionChange(float vel){
}

void Ground::notifyCollision(Solid* obj){
}

void Ground::setModelMatrix(){
	Point tr = this->dimensions.center();
	glTranslatef(tr.x1,tr.x2,tr.x3);
}

void Ground::setSubModelMatrix(const std::string& name){
	Point tr = this->dimensions.center();
	glTranslatef(tr.x1,tr.x2,tr.x3);
}

float Ground::calculateZ(float x,float y){
	const std::vector<float>& vertices = this->getObjectModel().vertices();
	float tileWidth = this->tileWidth();
	float tileHeight = this->tileHeight();
	unsigned int cols = this->cols();
	unsigned int rows = this->rows();
	Point xyPos = Point(x,y,0.0f);
	float z=0.0f;

	unsigned int tile_x = (unsigned int)floor(x/tileWidth);
	unsigned int tile_y = (unsigned int)floor(y/tileHeight);

	// la posicion la necesito relativa al centro del tile
	xyPos -= Point(tile_x*tileWidth+(tileWidth*0.5f),tile_y*tileHeight+(tileHeight*0.5f),0.0f);

	Point tile_vertices[4];
	tile_vertices[0] = Point(	vertices[(tile_x + tile_y*cols)*3],
								vertices[((tile_x + tile_y*cols))*3+1],
								vertices[((tile_x + tile_y*cols))*3+2]);

	tile_vertices[1] = Point(	vertices[(tile_x + (tile_y+1)*cols)*3],
								vertices[((tile_x + (tile_y+1)*cols))*3+1],
								vertices[((tile_x + (tile_y+1)*cols))*3+2]);

	tile_vertices[2] = Point(	vertices[((tile_x+1) + tile_y*cols)*3],
								vertices[(((tile_x+1) + tile_y*cols))*3+1],
								vertices[(((tile_x+1) + tile_y*cols))*3+2]);

	tile_vertices[3] = Point(	vertices[((tile_x+1) + (tile_y+1)*cols)*3],
								vertices[(((tile_x+1) + (tile_y+1)*cols))*3+1],
								vertices[(((tile_x+1) + (tile_y+1)*cols))*3+2]);

	// La normal a la diagonal del tile me permite ver en cual de los 2 triangulos
	// del tile esta parado el observador
	Point diagonal = tile_vertices[2]-tile_vertices[1];
	diagonal.x3=0;
	Point diagNorm = Point(0,0,1.0f).cross(diagonal);

	if(diagNorm*xyPos>=0.0f){
		// la posicion la necesito relativa al centor del mapa
		xyPos = Point(x,y,0.0f)-Point(cols*0.5*tileWidth,rows*0.5*tileHeight);
		Point triNorm = (tile_vertices[2]-tile_vertices[3]).cross(tile_vertices[1]-tile_vertices[3]);
		float d = triNorm*tile_vertices[2];
		// x,y ya son conocidas es donde esta parado el observador solo necesita ver a que coordenada z
		// en el triangulo corresponde ese x,y
		z = (d + ((-triNorm.x1)*xyPos.x1)+((-triNorm.x2)*xyPos.x2))/triNorm.x3;
	}else{
		// la posicion la necesito relativa al centor del mapa
		xyPos = Point(x,y,0.0f)-Point(cols*0.5*tileWidth,rows*0.5*tileHeight);
		Point triNorm = (tile_vertices[2]-tile_vertices[0]).cross(tile_vertices[1]-tile_vertices[0]);
		float d = triNorm*tile_vertices[2];
		// x,y ya son conocidas es donde esta parado el observador solo necesita ver a que coordenada z
		// en el triangulo corresponde ese x,y
		z = (d + ((-triNorm.x1)*xyPos.x1)+((-triNorm.x2)*xyPos.x2))/triNorm.x3;
	}

	return z;
}
